
#include "rviz_custom_plugins/utils/parser.h"
#include <QStringList>

namespace rviz_custom_plugins
{

namespace utils
{


static int limit(int i)
{
    if (i < 0) 
    {
        return 0;
    }
    if (i > 255) 
    {
        return 255;
    }
    return i;
}


QColor parseColor(const QString & colorStr)
{
    if (colorStr.indexOf(';') != -1) 
    {
        QStringList strings = colorStr.split(';');
        if (strings.size() >= 4) {
            bool r_ok = true;
            int r = strings[0].toInt(&r_ok);
            bool g_ok = true;
            int g = strings[1].toInt(&g_ok);
            bool b_ok = true;
            int b = strings[2].toInt(&b_ok);
            bool a_ok = true;
            int a = strings[3].toInt(&a_ok);
            if (r_ok && g_ok && b_ok && a_ok) {
                return QColor(limit(r), limit(g), limit(b), limit(a));
            }
        }
        return QColor();
    }

    QColor newColor;
    if (QColor::colorNames().contains(colorStr, Qt::CaseInsensitive) ||
        (colorStr.size() > 0 && colorStr[0] == '#' ))
    {
        newColor.setNamedColor(colorStr.toLower() );
    }
    return newColor;
}


geometry_msgs::msg::Point parsePoint(const QString & vectorStr)
{
    geometry_msgs::msg::Point point{};
    QStringList strings = vectorStr.split(';');
    if (strings.size() >= 3) {
        bool x_ok = true;
        int x = strings[0].toFloat(&x_ok);
        bool y_ok = true;
        int y = strings[1].toFloat(&y_ok);
        bool z_ok = true;
        int z = strings[2].toFloat(&z_ok);
        if (x_ok && y_ok && z_ok) {
            point.x = x;
            point.y = y;
            point.z = z;
            return point;
        }
    }
    return point;
    
}


}


}